/*
 * @Description: 
 * @Author: Sang Hao
 * @Date: 2021-09-15 17:11:30
 * @LastEditTime: 2021-10-27 11:43:00
 * @LastEditors: Sang Hao
 */

#include <Eigen/Dense>
#include <geometry_msgs/PoseWithCovarianceStamped.h>

#include "lidar_slam/publisher/key_frame_publisher.hpp"

namespace lidar_slam {
KeyFramePublisher::KeyFramePublisher(ros::NodeHandle& nh, 
	std::string topic_name,	std::string frame_id, int buff_size)
	: nh_(nh), frame_id_(frame_id) {
	publisher_ = nh_.advertise<geometry_msgs::PoseWithCovarianceStamped>(topic_name, buff_size);
}

/**
 * @description: 发布函数，包装到geometry_msgs::PoseWithCovarianceStamped中发布
 * 				key_frames，包装到了nav_msgs::Path中发布
 * @param  {*}
 * @return {*}
 */
void KeyFramePublisher::Publish(KeyFrame& key_frame) {
	geometry_msgs::PoseWithCovarianceStamped pose_stamped;

	ros::Time ros_time((float)key_frame.time);
	pose_stamped.header.stamp = ros_time;
	pose_stamped.header.frame_id = frame_id_;

	pose_stamped.pose.pose.position.x = key_frame.pose(0, 3);
	pose_stamped.pose.pose.position.y = key_frame.pose(1, 3);
	pose_stamped.pose.pose.position.z = key_frame.pose(2, 3);

	Eigen::Quaternionf q = key_frame.GetQuaternion();
	pose_stamped.pose.pose.orientation.x = q.x();
	pose_stamped.pose.pose.orientation.y = q.y();
	pose_stamped.pose.pose.orientation.z = q.z();
	pose_stamped.pose.pose.orientation.w = q.w();
	
	pose_stamped.pose.covariance[0] = (double)key_frame.index;
	
	publisher_.publish(pose_stamped);
}


bool KeyFramePublisher::HasSubscribers() {
	return publisher_.getNumSubscribers() != 0;
}
}